﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using haptic.src.engine.debug;

namespace haptic.src.engine.collisions
{
    [Serializable]
    public class OOBBCollider : ICollider
    {
        #region Constructor
        /// <summary>
        /// only for serialization
        /// </summary>
        public OOBBCollider()
        {

        }
        public OOBBCollider(Vector2 vCenter, Vector2 vRadius)
        {
            Set(vCenter, vRadius);
        }
        public OOBBCollider(Vector2 vCenter, Vector2 vSize, Vector2 vOldCenter)
        {
            Set(vCenter, vSize, vOldCenter);
        }
        public OOBBCollider(Vector2 vCenter, Vector2 vSize, Vector2 vOldCenter, Vector2 vOldSize)
        {
            Set(vCenter, vSize, vOldCenter, vOldSize);
        }
        public OOBBCollider(OOBBCollider oC)
        {
            m_vCenter = oC.m_vCenter;
            m_vExtents = oC.m_vExtents;
            m_vOldHalfSize = oC.m_vOldHalfSize;
            m_vOldCenter = oC.m_vOldCenter;
        }
        public ICollider Clone()
        {
            return new OOBBCollider(this);
        }

        #endregion
        #region Edit
        public void Set(Vector2 vCenter, Vector2 vRadius)
        {
            m_vCenter = vCenter;
            m_vExtents = vRadius;
            m_vOldCenter = vCenter;
            m_vOldHalfSize = m_vExtents;
            m_vAxes[0] = Vector2.UnitX;
            m_vAxes[1] = Vector2.UnitY;
        }
        public void Set(Vector2 vCenter, Vector2 vRadius, Vector2 vOldCenter)
        {
            m_vCenter = vCenter;
            m_vExtents = vRadius;
            m_vOldCenter = vOldCenter;
            m_vOldHalfSize = m_vExtents;
            m_vAxes[0] = Vector2.UnitX;
            m_vAxes[1] = Vector2.UnitY;
        }
        public void Set(Vector2 vCenter, Vector2 vRadius, Vector2 vOldCenter, Vector2 vOldRadius)
        {
            m_vCenter = vCenter;
            m_vExtents = vRadius;
            m_vOldCenter = vOldCenter;
            m_vOldHalfSize = vOldRadius;
            m_vAxes[0] = Vector2.UnitX;
            m_vAxes[1] = Vector2.UnitY;
        }
        public void SetCenter(Vector2 vCenter)
        {
            m_vCenter = vCenter;
        }
        public void SetAxes(Vector2 vXAxis, Vector2 vYAxis)
        {
            m_vAxes[0] = vXAxis;
            m_vAxes[1] = vYAxis;
            m_vAxes[0].Normalize();
            m_vAxes[1].Normalize();
        }
        public void SetAxes(Matrix mMat)
        {
            m_vAxes[0].X = mMat.M11;
            m_vAxes[0].Y = mMat.M21;
            m_vAxes[1].X = mMat.M12;
            m_vAxes[1].Y = mMat.M22;
            m_vAxes[0].Normalize();
            m_vAxes[1].Normalize();
        }
        #endregion
        #region Collisions
        public bool IsInside(Vector2 vPos)
        {
            Vector2 vRelPos = vPos - m_vCenter;
            return (vRelPos.X >= 0.0f && vRelPos.Y >= 0.0f && vRelPos.X <= m_vExtents.X && vRelPos.Y <= m_vExtents.Y);
        }
        public bool IsInside(ICollider oCollider)
        {
            if (oCollider.GetType() == typeof(OOBBCollider))
            {
                OOBBCollider oC = (OOBBCollider)oCollider;
                return IsInside(oC.m_vCenter) && IsInside(oC.m_vCenter + oC.m_vExtents);
            }
            return false;
        }

        public bool IsColliding(ICollider oCollider, CollisionHit oCollisionHit)
        {
            if (oCollider == null)
                return false;
            if (oCollider.GetType() == typeof(OOBBCollider))
            {
                OOBBCollider oC = (OOBBCollider)oCollider;
                float ra, rb;
                Vector2[] R = new Vector2[2];
                Vector2[] AbsR = new Vector2[2];

                // Compute rotation matrix expressing b in current collider's coordinate frame
                R[0].X = Vector2.Dot(m_vAxes[0], oC.m_vAxes[0]);
                R[0].Y = Vector2.Dot(m_vAxes[1], oC.m_vAxes[0]);
                R[1].X = Vector2.Dot(m_vAxes[0], oC.m_vAxes[1]);
                R[1].Y = Vector2.Dot(m_vAxes[1], oC.m_vAxes[1]);

                // Compute translation vector t
                Vector2 t = oC.m_vCenter - m_vCenter;

                // Bring translation into current collider's coordinate frame
                t = new Vector2(Vector2.Dot(t, m_vAxes[0]), Vector2.Dot(t, m_vAxes[1]));

                // Compute common subexpressions. Add in an epsilon term to
                // counteract arithmetic errors when two edges are parallel and
                // their cross product is (near) null (see text for details)
                float fEpsilon = 0.001f;
                AbsR[0].X = Math.Abs(R[0].X) + fEpsilon;
                AbsR[0].Y = Math.Abs(R[0].Y) + fEpsilon;
                AbsR[1].X = Math.Abs(R[1].X) + fEpsilon;
                AbsR[1].Y = Math.Abs(R[1].Y) + fEpsilon;

                // Axis L = AX
                float fSepAX;
                {
                    ra = m_vExtents.X;
                    rb = oC.m_vExtents.X * AbsR[0].X + oC.m_vExtents.Y * AbsR[0].Y;
                    fSepAX = Math.Abs(t.X) - (ra + rb);
                    if (fSepAX > 0.0f) return false;
                }

                // Axis L = AY
                float fSepAY;
                {
                    ra = m_vExtents.Y;
                    rb = oC.m_vExtents.X * AbsR[1].X + oC.m_vExtents.Y * AbsR[1].Y;
                    fSepAY = Math.Abs(t.Y) - (ra + rb);
                    if (fSepAY > 0.0f) return false;
                }

                // Axis L = BX
                float fSepBX;
                {
                    ra = m_vExtents.X * AbsR[0].X + m_vExtents.Y * AbsR[0].Y;
                    rb = oC.m_vExtents.X;
                    fSepBX = Math.Abs(t.X * R[0].X + t.Y * R[0].Y) - (ra + rb);
                    if (fSepBX > 0.0f) return false;
                }

                // Axis L = BY
                float fSepBY;
                {
                    ra = m_vExtents.X * AbsR[1].X + m_vExtents.Y * AbsR[1].Y;
                    rb = oC.m_vExtents.Y;
                    fSepBY = Math.Abs(t.X * R[1].X + t.Y * R[1].Y) - (ra + rb);
                    if (fSepBY > 0.0f) return false;
                }

                if (oCollisionHit != null)
                {
                    float fTLength = t.Length();
                    if (fTLength != 0.0f)
                    {
                        oCollisionHit.m_vNormal = -t * (1.0f / fTLength);
                        //oCollisionHit.m_fSeparation = Math.Min(Math.Min(fSepAX, fSepAY), Math.Min(fSepBX, fSepBY));
                    }
                    else
                    {
                        oCollisionHit.m_vNormal = Vector2.UnitX;
                        //oCollisionHit.m_fSeparation = Math.Max(Math.Abs(oC.m_vExtents.X), Math.Abs(oC.m_vExtents.Y));
                    }
                }

                // Since no separating axis found, the OBBs must be intersecting
                return true;
            }
            return false;
        }

        public bool IsColliding(ICollider oCollider)
        {
            return IsColliding(oCollider, null);
        }

        #endregion

        public BoundingBox GetBoundingBox()
        {
            return new BoundingBox(m_vCenter, m_vExtents);
        }

        #region Draw
        public void DrawDebug(DebugRenderer oDebug)
        {
            oDebug.DrawBox(m_vCenter - m_vExtents, m_vExtents + m_vExtents, Color.Red);
        }
        #endregion

        #region Attributes
        private Vector2 m_vCenter;
        public Vector2 vCenter { get { return m_vCenter; } }
        private Vector2 m_vOldCenter;
        private Vector2 m_vExtents;
        public Vector2 vExtents { get { return m_vExtents; } }
        private Vector2 m_vOldHalfSize;
        private Vector2[] m_vAxes = new Vector2[2];
        #endregion
    }
}
